/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package com.grt192.mechanism.breakaway;

import com.grt192.actuator.GRTJaguar;
import com.grt192.actuator.GRTServo;
import com.grt192.core.Command;
import com.grt192.core.Mechanism;
import com.grt192.core.Sensor;
import com.grt192.sensor.GRTSwitch;

/**
 *
 * @author alvin, nikhil
 */
public class Kicker extends Mechanism {
    //angle positions for servo

    public static final double SERVO_UP = 100;
    public static final double SERVO_DOWN = 15;
    public static final double KICK_SPEED = 1.0;
    //boolean to tell if the kicker is kicking at a given moment
    private boolean kicking;
    //boolean to tell if the guillotine is up (true) or down (false)
    private boolean guillotineUp;

    public Kicker(GRTJaguar jaguar, GRTServo servo, GRTSwitch limitSwitch) {
        //start jaguar, servo, limitSwitch
        kicking = guillotineUp = false;
        jaguar.start();
        servo.start();
        limitSwitch.start();

        //add actuators of jaguar, limitSwitch, and servo with string tags
        addActuator("Motor", jaguar);
        addSensor("LimitSwitch", limitSwitch);
        addActuator("Servo", servo);
    }

    public boolean hitSwitch() {
        return getSensor("LimitSwitch").getState("State") == Sensor.TRUE;
    }

    public void setSpeed(double speed) {
        getActuator("Motor").enqueueCommand(new Command(speed));
    }

    public void turnServo(double angle) {
        if (0 <= angle && angle <= 180) {
            getActuator("Servo").enqueueCommand(new Command(angle));
        }
    }

    public void reverse(){
        setSpeed(KICK_SPEED);
        kicking = false;
    }

    public void kick() {
        setSpeed(-KICK_SPEED);
        kicking = true;
    }

    public void stopKick() {
        setSpeed(0);
        kicking = false;
    }

    public boolean isKicking() {
        return kicking;
    }

    public void raiseGuillotine() {
        turnServo(SERVO_UP);
        guillotineUp = true;
    }

    public void lowerGuillotine() {
        turnServo(SERVO_DOWN);
        guillotineUp = false;
    }

    public boolean isGuillotineUp() {
        return guillotineUp;
    }
}
